#include "animation.h"

#include <mymath.h>

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



void Animation::transform_ModelSet_MESHES( int frameNumber_XwrisOffset )
{


                ////////////////////////////////////////////////////////////////////////////////////////////
                ////////////////////////////////////////////////////////////////////////////////////////////
                if (  (frameNumber_XwrisOffset < 0)  ||  (frameNumber_XwrisOffset >= totalAllignedFrames)  )
                {
                    return;
                }
                ////////////////////////////////////////////////////////////////////////////////////////////
                ////////////////////////////////////////////////////////////////////////////////////////////

                /////////////////////////////////////////////////////////
                /////////////////////////////////////////////////////////
                int frameNumber = frameNumber_XwrisOffset + motionOffset;
                /////////////////////////////////////////////////////////
                /////////////////////////////////////////////////////////

                //////////////////////////////////////////////////////////////////////////////////////////
                //////////////////////////////////////////////////////////////////////////////////////////

                Eigen::MatrixXd mat_0(         4,4);
                Eigen::MatrixXd mat_c(         4,4);
                Eigen::MatrixXd mat_C_0inv(    4,4);
                Eigen::MatrixXd vertex(        4,1);
                Eigen::MatrixXd weightedVertex(4,1);

                //////////////////////////////////////////////////////////////////////////////////////////
                //////////////////////////////////////////////////////////////////////////////////////////

                for (int modelID=0; modelID<modelSet.totalModels; modelID++)
                {

                            //////////////////////////////////////////////////////////////
                            //////////////////////////////////////////////////////////////
                            Model *pModel = &(modelSet.models[modelID]);
                            //////////////////////////////////////////////////////////////
                            //////////////////////////////////////////////////////////////
                            pModel->mesh.verticesWeighted.fill( Eigen::Vector3d::Zero() );
                            //////////////////////////////////////////////////////////////
                            //////////////////////////////////////////////////////////////

                            for (int skkk=0; skkk<pModel->totalSkinningBones; skkk++)
                            {

                                                            ///////////////////////////////////////////////////////////////////////////
                                                            ///////////////////////////////////////////////////////////////////////////
                                                            int bbb = pModel->lookup_SkinningBone_to_Bones_ID[skkk];
                                                            ///////////////////////////////////////////////////////////////////////////
                                                            ///////////////////////////////////////////////////////////////////////////
                                                            mat_0 << movingModels[modelID].motionFrames[     0     ].bones[bbb].RT_4x4;
                                                            mat_c << movingModels[modelID].motionFrames[frameNumber].bones[bbb].RT_4x4;
                                                            ///////////////////////////////////////////////////////////////////////////
                                                            ///////////////////////////////////////////////////////////////////////////
                                                            mat_C_0inv = mat_c * mat_0.inverse();
                                                            ///////////////////////////////////////////////////////////////////////////
                                                            ///////////////////////////////////////////////////////////////////////////

                                                            for (int vvv=0; vvv<pModel->totalVertices; vvv++)
                                                            {

                                                                    double skinnWeight = pModel->skin.skinnedVertices[vvv].skinWeights[skkk];


                                                                    if  (skinnWeight > 0)
                                                                    {
                                                                            vertex << pModel->mesh.vertices[vvv](0),
                                                                                      pModel->mesh.vertices[vvv](1),
                                                                                      pModel->mesh.vertices[vvv](2),
                                                                                      1;

                                                                            weightedVertex = skinnWeight * mat_C_0inv * vertex;

                                                                            pModel->mesh.verticesWeighted[vvv](0) += weightedVertex(0);
                                                                            pModel->mesh.verticesWeighted[vvv](1) += weightedVertex(1);
                                                                            pModel->mesh.verticesWeighted[vvv](2) += weightedVertex(2);
                                                                    }

                                                            } // vvv

                            } // for (int skkk=0; skkk<modelSet.models[modelID].totalSkinningBones; skkk++)


                            ///////////////////////////////////////////
                            ///////////////////////////////////////////
                            modelSet.models[modelID].compute_NORMALS();
                            ///////////////////////////////////////////
                            ///////////////////////////////////////////


                            //////////////////////////////////
                            //////////////////////////////////
                            pModel->compute_Centroid4D_curr();
                            //////////////////////////////////
                            //////////////////////////////////


                } // for (int modelID=0; modelID<modelSet.totalModels; modelID++)



                ///////////////////////////////////
                ///////////////////////////////////
                modelSet.compute_Centroid4D_curr();
                ///////////////////////////////////
                ///////////////////////////////////


                ////////////////////////////////////////////////
                ////////////////////////////////////////////////
                modelSet.compute_VerticesOfInterest_Centroids();
                ////////////////////////////////////////////////
                ////////////////////////////////////////////////


}




/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////




void Animation::transform_ModelSet_SKELETONS( int frameNumber_XwrisOffset )
{


        ////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////
        if (  (frameNumber_XwrisOffset < 0)  ||  (frameNumber_XwrisOffset >= totalAllignedFrames)  )
        {
            return;
        }
        ////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////

        /////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////
        int frameNumber = frameNumber_XwrisOffset + motionOffset;
        /////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////

        ////////////////////////////////////////////////////////////////////////////////////////////
        ////////////////////////////////////////////////////////////////////////////////////////////


        int currrAdd;
        int childAdd;


        for (int modelID=0; modelID<modelSet.totalModels; modelID++)
        {

                modelSet.models[modelID].skeleton.startingBone = modelSet.models[modelID].skeleton.bones[ 0 ];


                if (modelSet.models[modelID].skeleton.startingBone.childrenVector.size() > 0)
                {

                        for (int j=0; j<modelSet.models[modelID].skeleton.startingBone.childrenVector.size(); j++)
                        {

                                        currrAdd = modelSet.models[modelID].skeleton.startingBone.addressSeListaBones;   // diefthinsi tou parent  stin oliki oliki lista me ola ta Bones !!!
                                        childAdd = modelSet.models[modelID].skeleton.startingBone.childrenVector[j];     // diefthinsi tou paidiou stin oliki oliki lista me ola ta Bones !!!

                                        ///////////////////////////////////////////////////////////
                                        /////////    Wrists/obects - RBM_Joint  ///////////////////
                                        ///////////////////////////////////////////////////////////
                                        calculate_Bones_using_RT( modelID, frameNumber, currrAdd );
                                        ///////////////////////////////////////////////////////////
                                        ///////////////////////////////////////////////////////////

                                        modelSet.models[modelID].skeleton.currentBone = modelSet.models[modelID].skeleton.bones[childAdd];

                                        while (modelSet.models[modelID].skeleton.currentBone.childrenVector.size() > 0)
                                        {

                                                currrAdd = modelSet.models[modelID].skeleton.currentBone.addressSeListaBones;
                                                childAdd = modelSet.models[modelID].skeleton.currentBone.childrenVector[0];

                                                ///////////////////////////////////////////////////////////
                                                calculate_Bones_using_RT( modelID, frameNumber, currrAdd );
                                                ///////////////////////////////////////////////////////////

                                                modelSet.models[modelID].skeleton.currentBone = modelSet.models[modelID].skeleton.bones[childAdd];

                                        }
                                        if (modelSet.models[modelID].skeleton.currentBone.childrenVector.size() == 0 )
                                        {
                                                currrAdd = modelSet.models[modelID].skeleton.currentBone.addressSeListaBones;

                                                ///////////////////////////////////////////////////////////
                                                calculate_Bones_using_RT( modelID, frameNumber, currrAdd );
                                                ///////////////////////////////////////////////////////////
                                        }

                        }

                }
                else
                {
                        currrAdd = modelSet.models[modelID].skeleton.startingBone.addressSeListaBones;

                        calculate_Bones_using_RT( modelID, frameNumber, currrAdd );
                }

        } // for (int modelID=0; modelID<tracker->sequence.posedAnimations[animNumb].modelSet.totalModels; modelID++)

}





void Animation::calculate_Bones_using_RT( int modelID, int frameNumber, int currrAdd ) // ME Offset ;)
{

                            Eigen::MatrixXd curVect_S( 4,1);
                            Eigen::MatrixXd mat_S(     4,4);
                            Eigen::MatrixXd leafVectt( 4,1);

                            int skkk = modelSet.models[modelID].skeleton.bones[currrAdd].addressSeListaSKINNING;  // currrAdd - input from above!!!

                            if (skkk>-1)
                            {

                                    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                                                                                    leafVectt << modelSet.models[modelID].skeleton.currentBone.length, 0, 0, 1;
                                    if (skkk == 0  &&  modelSet.models[modelID].has_OnlySkin)       leafVectt <<                                                    0, 0, 0, 1;
                                    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                    mat_S << movingModels[modelID].motionFrames[ frameNumber ].bones[currrAdd].RT_4x4;

                                    curVect_S << mat_S(0,3), mat_S(1,3), mat_S(2,3), 1;

                                    /////////////////////////////////////////////////////////////////////////////////////////////////
                                    /////////////////////////////////////////////////////////////////////////////////////////////////
                                    modelSet.models[modelID].skinningSkeleton.skinningBones[skkk].bone_Start_4x1 =         curVect_S;
                                    modelSet.models[modelID].skinningSkeleton.skinningBones[skkk].bone_Leaff_4x1 = mat_S * leafVectt;
                                    /////////////////////////////////////////////////////////////////////////////////////////////////
                                    /////////////////////////////////////////////////////////////////////////////////////////////////

                            }

}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////






void Animation::calculate_RotationAxes( int frameNumber_XwrisOffset )
{

        /////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////
        if (  (frameNumber_XwrisOffset < 0)  ||  (frameNumber_XwrisOffset >= totalAllignedFrames)  )
        {
            return;
        }
        /////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////
        int frameNumber = frameNumber_XwrisOffset + motionOffset;
        /////////////////////////////////////////////////////////////////////////////////////////////
        /////////////////////////////////////////////////////////////////////////////////////////////


        for (int modelID=0; modelID<modelSet.totalModels; modelID++)
        {

                    if ( modelSet.models[modelID].articulationSkel.rotationAxes.size()   !=    modelSet.models[modelID].totalSkinningBones )
                    {
                         modelSet.models[modelID].articulationSkel.rotationAxes.resize(        modelSet.models[modelID].totalSkinningBones );
                    }

                    Eigen::MatrixXd mat_3x3(3,3);

                    for (int skkk=0; skkk<modelSet.models[modelID].totalSkinningBones; skkk++)
                    {

                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    Custom_3DoF_Axis *curr3DoF_RotAxes = &(modelSet.models[modelID].articulationSkel.rotationAxes[skkk]);
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                    int boneAdd = modelSet.models[modelID].lookup_SkinningBone_to_Bones_ID[skkk];

                                    curr3DoF_RotAxes->location << movingModels[modelID].motionFrames[frameNumber].bones[boneAdd].T(0),    // @ RIGGING_POSE !!!!!!!!!!!!!!!!!!!!!!!
                                                                  movingModels[modelID].motionFrames[frameNumber].bones[boneAdd].T(1),    // @ RIGGING_POSE !!!!!!!!!!!!!!!!!!!!!!!
                                                                  movingModels[modelID].motionFrames[frameNumber].bones[boneAdd].T(2);    // @ RIGGING_POSE !!!!!!!!!!!!!!!!!!!!!!!

                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                    // For Axes DIR Creation see SetLimits !!!!!!!!!! -> to kick out dummy axes

                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                    mat_3x3 = movingModels[modelID].motionFrames[ frameNumber ].bones[ boneAdd ].R;

                                    for (int axx=0; axx<curr3DoF_RotAxes->axe.size(); axx++)
                                    {
                                        curr3DoF_RotAxes->axe[axx].dir      = mat_3x3                         * curr3DoF_RotAxes->axe[axx].dir_iniV;
                                        curr3DoF_RotAxes->axe[axx].dir_draw = curr3DoF_RotAxes->location + 10 * curr3DoF_RotAxes->axe[axx].dir;
                                    }

                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                    curr3DoF_RotAxes->name_joint_OR_bone  = modelSet.models[modelID].skeleton.bones[   modelSet.models[modelID].lookup_SkinningBone_to_Bones_ID[skkk]   ].name;
                                    curr3DoF_RotAxes->ID_se_Bones         =                                            modelSet.models[modelID].lookup_SkinningBone_to_Bones_ID[skkk];
                                    curr3DoF_RotAxes->ID_se_SkinningBones =                                                                                                     skkk;

                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                    }  // for (int skkk=0; skkk<modelSet.models[modelID].totalSkinningBones; skkk++)

        }  // for (int modelID=0; modelID<modelSet.totalModels; modelID++)

}




